#ifndef _CHEADPOSE_H_
#define _CHEADPOSE_H_

#include <buola/mat.h>
#include <robot/pacohead/enums.h>

namespace buola { namespace robot { namespace pacohead {

class CHeadPose
{
public:
    CHeadPose()
    {
    }
    
    CHeadPose(const CHeadPose &pPose)
    {
        for(int i=0;i<7;i++)
            mJoints[i]=pPose.mJoints[i];
    }

    CHeadPose(double pJoints[7])
    {
        for(int i=0;i<7;i++)
            mJoints[i]=pJoints[i];
    }

    CHeadPose(const std::vector<double> &pJoints)
    {
        for(int i=0;i<7;i++)
            mJoints[i]=pJoints[i];
    }

    double &NeckPitch()	{	return mJoints[JOINT_NECK_PITCH];	}
    double &NeckRoll()	{	return mJoints[JOINT_NECK_ROLL];	}
    double &NeckYaw()	{	return mJoints[JOINT_NECK_YAW];		}
    double &NeckTilt()	{	return mJoints[JOINT_NECK_TILT];	}
    double &EyesTilt()	{	return mJoints[JOINT_EYES_TILT];	}
    double &EyesRight()	{	return mJoints[JOINT_EYES_RIGHT];	}
    double &EyesLeft()	{	return mJoints[JOINT_EYES_LEFT];	}

    const double &NeckPitch() const	{	return mJoints[JOINT_NECK_PITCH];	}
    const double &NeckRoll() const	{	return mJoints[JOINT_NECK_ROLL];	}
    const double &NeckYaw() const	{	return mJoints[JOINT_NECK_YAW];		}
    const double &NeckTilt() const	{	return mJoints[JOINT_NECK_TILT];	}
    const double &EyesTilt() const	{	return mJoints[JOINT_EYES_TILT];	}
    const double &EyesRight() const	{	return mJoints[JOINT_EYES_RIGHT];	}
    const double &EyesLeft() const	{	return mJoints[JOINT_EYES_LEFT];	}

    double &operator[](int pJ)				{	return mJoints[pJ];	}
    const double &operator[](int pJ) const	{	return mJoints[pJ];	}

    double *GetJoints()						{	return mJoints;		}

    void InitHeadToCamerasMatrix(mat::CMat34d &pMatrix);
    void InitCamerasToHeadMatrix(mat::CMat34d &pMatrix);

    mat::CVec3d HeadToCameras(const mat::CVec3d &pHead);
    mat::CVec3d CamerasToHead(const mat::CVec3d &pCam);
    double GetLookAtDist();

    double InvKinematics(const mat::CVec3d &pV,int pIter=5);

    void Normalize();

    void Limit(const CHeadPose &pMin,const CHeadPose &pMax,double pFactor);

    void TestZero(double pE);

    CHeadPose &operator=(const CHeadPose &pPose)
    {
        for(int i=0;i<7;i++)
            mJoints[i]=pPose.mJoints[i];

        return *this;
    }


private:
    void ComputeErrors(const mat::CVec3d &pTarget,mat::CVec3d &pErrors);
    void ComputeJacobianColumn(const mat::CVec3d &pError,
                    const mat::CVec3d &pTarget,
                mat::CMat34d &pJacobian,int pJoint,int pCol);
    void ComputeJacobian(mat::CVec3d &pError,
                    const mat::CVec3d &pTarget,mat::CMat34d &pJacobian);
    void SolveDeltaAngles(const mat::CVec3d &pError,const mat::CMat34d &pJacobian,
                                            mat::CVec3d &pDelta);

private:
    double mJoints[7];    
};

template<typename tChar>
inline io::CTextWriter &operator<<(io::CTextWriter &pWriter,const CHeadPose &pP)
{
    for(int i=0;i<7;i++)
    {
        pWriter << pP[i] << ' ';
        pWriter << "\n";
    }
    return pWriter;
}

/*namespace pacohead*/ } /*namespace buola*/ } /*namespace robot*/ }

#endif
